/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * $Id: userif.c 44 2014-04-21 21:34:01Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <stdio.h>

// Maxim CMSIS includes
#include "config.h"
#include "errors.h"
#include "uart_regs.h"

// RTOS includes
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

/***** Definitions *****/

/***** Global Variables *****/
extern int compass_exists;
extern int compass_heading;

/***** File Scope Data *****/
static xQueueHandle xQueue;
static TaskHandle_t xDiagHandle;

/***** Function Prototypes *****/
static void display_menu(void);
static void userif_task(void * pvParameters);
static void diagnostic_task(void * pvParameters);

//******************************************************************************
int userif_init(void)
{
  // Enable the UART receive interrupt for each character
  MXC_UART0->ctrl = (MXC_UART0->ctrl & ~MXC_F_UART_CTRL_RXTHD) | (1 << MXC_F_UART_CTRL_RXTHD_POS);
  MXC_UART0->int_en |= MXC_F_UART_INTEN_FFRXIE;
  NVIC_EnableIRQ(UART0_IRQn);

  // Create a queue for passing characters from the ISR
  xQueue = xQueueCreate(2, sizeof(char));
  if (xQueue == NULL) {
    printf("ERROR: Failed to create User I/F queue\n");
    return E_NONE_AVAIL;
  }

  // Create the control task
  if (xTaskCreate(userif_task, (const char * const)"userif", 200, NULL, 1, NULL) != pdPASS) {
    return E_NONE_AVAIL;
  }

  // Create the diagnostic task
  if (xTaskCreate(diagnostic_task, (const char * const)"diag", 1024, NULL, 1, &xDiagHandle) != pdPASS) {
    return E_NONE_AVAIL;
  }

  return E_NO_ERROR;
}

//******************************************************************************
void userif_task(void * pvParameters)
{
  char byte;
  unsigned int resume;

  if (xQueue == NULL)
    vTaskSuspend(NULL);

  while(1) {
    // Wait indefinitely for a message in the queue
    if (xQueueReceive(xQueue, &byte, portMAX_DELAY) != pdTRUE) {
      printf("ERROR: xQueueReceive error in User I/F\n");
    }

    vTaskSuspend(xDiagHandle);
    display_menu();

    resume = 0;
    do {
      // Wait indefinitely for a message in the queue
      if (xQueueReceive(xQueue, &byte, portMAX_DELAY) == pdTRUE) {
        switch (byte) {
          case 'c':
            printf("Continuing...\n");
            resume = 1;
            break;
          case 0x1B:
            printf("Continuing...\n");
            resume = 1;
            break;
          default:
            display_menu();
            break;
        }
      } else {
        printf("ERROR: xQueueReceive error in User I/F\n");
        break;
      }
    } while (!resume);

    vTaskResume(xDiagHandle);
  }
}

//******************************************************************************
void UART0_IRQHandler(void)
{
  char byte;

  // Safely clear the interrupt flag
  MXC_UART0->int_stat = ~MXC_F_UART_INTSTAT_FFRXIS;

  // Get the received byte and send it to the User I/F task
  byte = MXC_UART0->data;
  if (xQueueSendFromISR(xQueue, &byte, NULL) != pdTRUE) {
    printf("ERROR: xQueueSend() failed\n");
  }
}

//******************************************************************************
void display_menu(void)
{
  printf("\n### MENU ###\n");
#if 0
  if (motor_mode == FIXED_VALUE) {
    printf("  m - Set motor value (0x%02x 0x%02x)\n", motor_value[LEFT], motor_value[RIGHT]);
    printf("  s - Set speed\n");
  } else {
    printf("  m - Set motor value\n");
    printf("  s - Set speed (%u %u)\n", speed_setpoint[LEFT], speed_setpoint[RIGHT]);
  }
  printf("  f - Go forward (%u, %u)\n", get_tripremaining_value(LEFT), get_tripremaining_value(RIGHT));
#endif
  printf("  t - Turn\n");
  printf("  r - Rotate\n");
  printf("  o - Coast\n");
  printf("  l - Display speed Log\n");
  printf("  p - Display Position log\n");
  printf("  y - Display encoder log\n");
  printf("  z - Display first encoder log\n");
  printf("  e - Erase logs\n");
  printf("  a - Calibrate compass\n");
  printf("  d - Distance log\n");
  printf("  q - Run TRIPQ\n");
  printf("  c - Continue\n");
}

//*****************************************************************************
// Perform an action every 10 ticks.
void diagnostic_task(void * pvParameters)
{
  TickType_t xLastWakeTime, xCurrTime;
  const TickType_t xFrequency = 400 / portTICK_PERIOD_MS;

  // Initialise the xLastWakeTime variable with the current time.
  xLastWakeTime = xTaskGetTickCount();

  while(1) {

    // Check the current time in case this task was suspended
    xCurrTime = xTaskGetTickCount();
    if (xCurrTime > (xLastWakeTime + xFrequency)) {
      xLastWakeTime = xCurrTime;
    }

    // Wait for the next cycle.
    vTaskDelayUntil(&xLastWakeTime, xFrequency);

    printf("diag @%u", (unsigned int)xLastWakeTime);

    if (compass_exists) {
      printf(" : %3d", compass_heading / 10);
    }

    printf("\n");
  }
}

